#include <rcl_interfaces/msg/floating_point_range.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <tf2_ros/transform_listener.h>

#include <chrono>
#include <cmath>
#include <memory>
#include <string>

#include "driver.h"

namespace lidar209_driver
{

lidar209Driver::lidar209Driver(const rclcpp::NodeOptions & options)
  : rclcpp::Node("lidar209_driver_node", options),
    diagnostics_(this, 0.2)
{

  std::string devip = this->declare_parameter("device_ip", std::string(""));
  bool gps_time = this->declare_parameter("gps_time", false);

  rcl_interfaces::msg::ParameterDescriptor offset_desc;
  offset_desc.name = "time_offset";
  offset_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
  offset_desc.description = "Time offset";
  rcl_interfaces::msg::FloatingPointRange offset_range;
  offset_range.from_value = -1.0;
  offset_range.to_value = 1.0;
  offset_desc.floating_point_range.push_back(offset_range);
  config_.time_offset = this->declare_parameter("time_offset", 0.0, offset_desc);


  config_.enabled = this->declare_parameter("enabled", true);
  bool read_once = this->declare_parameter("read_once", false);
  bool read_fast = this->declare_parameter("read_fast", false);
  double repeat_delay = this->declare_parameter("repeat_delay", 0.0);
  config_.frame_id = this->declare_parameter("frame_id", std::string("czlidar"));
  config_.model = this->declare_parameter("model", std::string("L209-QS88"));
  config_.rpm = this->declare_parameter("rpm", 600.0);
  std::string dump_file = this->declare_parameter("pcap", std::string(""));
  //double cut_angle = this->declare_parameter("cut_angle", 2.0 * M_PI);
  int udp_port = this->declare_parameter("port", static_cast<int>(DATA_PORT_NUMBER));
  //config_.cut_time = this->declare_parameter("cut_time", 1.0);
  int cut_time = this->declare_parameter("cut_time", 1);
  config_.cut_time = cut_time;

  future_ = exit_signal_.get_future();

  double packet_rate;                   // packet frequency (Hz)
  std::string model_full_name;
  if (config_.model == "L209-1")
  {
    packet_rate = 2803.0;
    model_full_name = "ZhuanXiang"+ config_.model;
  }
  else if (config_.model == "L209-2")
  {
    packet_rate = 7600.0;
    model_full_name = "ChaoWu"+ config_.model;
  }
  else if (config_.model == "L209-72")
  {
    packet_rate = 6862.0;
    model_full_name = "ChaoWu"+ config_.model;
  }
  else if (config_.model == "L209-QS88")
  {
    packet_rate = 3904.0;
    model_full_name = "CZLidar"+ config_.model;
  }  else if (config_.model == "L209-QS176"|| config_.model == "L209-ZW-QS176" )
  {
    //packet_rate = 7810.0;
    packet_rate = this->declare_parameter("packet_rate", 3904);

    model_full_name = "CZLidar"+ config_.model;
  }
  else {
    throw std::runtime_error("Unknown LIDAR model: " + config_.model);
  }


  std::string deviceName(std::string("lidar_209 ") + model_full_name);

  //motor speed

//  RCLCPP_INFO(this->get_logger(), "%s rotating at %f RPM", deviceName.c_str(), config_.rpm);

  double frequency = (config_.rpm / 60.0);     // expected Hz rate

  // default number of packets for each scan is a single revolution
  config_.npackets = static_cast<int>(std::ceil(packet_rate / frequency));

//  RCLCPP_INFO(this->get_logger(), "publishing %d packets per scan", config_.npackets);

 /* if (cut_angle < 0.0)
  {
    RCLCPP_INFO(this->get_logger(), "Cut at specific angle feature deactivated.");
  }
  else if (cut_angle < (2*M_PI))
  {
    RCLCPP_INFO(
          this->get_logger(), "Cut at specific angle feature activated. "
                              "Cutting lidar209 points always at %f rad.", cut_angle);
  }
  else
  {
    RCLCPP_ERROR(
          this->get_logger(), "cut_angle parameter is out of range."
                              "Allowed range is between 0.0 and 2*PI or negative values to deactivate this feature.");
    cut_angle = -0.01;
  }

  // Convert cut_angle from radian to one-hundredth degree,
  // which is used in lidar209 packets
  config_.cut_angle = static_cast<int>((cut_angle * 360 / (2 * M_PI)) * 100);*/


  // initialize diagnostics
  diagnostics_.setHardwareID(deviceName);
  const double diag_freq = packet_rate/config_.npackets;
  diag_max_freq_ = diag_freq;
  diag_min_freq_ = diag_freq;
//  RCLCPP_INFO(this->get_logger(), "expected frequency: %.3f (Hz)", diag_freq);

  diag_topic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
        "lidar209_packets", diagnostics_, diagnostic_updater::FrequencyStatusParam(
          &diag_min_freq_, &diag_max_freq_, 0.1, 10),
        diagnostic_updater::TimeStampStatusParam());

  // open lidar209 input device or file
  if (!dump_file.empty())                  // have PCAP file?
  {
    // read data from packet capture file
    input_ = std::make_unique<lidar209_driver::InputPCAP>(
          this, devip, udp_port, packet_rate,
          dump_file, read_once, read_fast, repeat_delay);
  }
  else
  {
    // read data from live socket
    input_ = std::make_unique<lidar209_driver::InputSocket>(this, devip, udp_port, gps_time);
  }

  // raw packet output topic

  if (config_.model == "L209-1")
  {
    //  output_ =        node.advertise<lidar209_msgs::lidar209Scan>("lidar209_packets", 5);
  }
  else if (config_.model == "L209-2")
  {
    // output_ =        node.advertise<lidar209_msgs::lidar209Scan>("lidar209_packets", 5);
  }
  else if (config_.model == "L209-72")
  {
    // output_ =        node.advertise<lidar209_msgs::lidar209Scan_CH72>("lidar209_packets", 5);
  }

  else if (config_.model == "L209-QS88" || config_.model == "L209-QS176" || config_.model == "L209-ZW-QS176" )
  {
    output_ =
        this->create_publisher<lidar209_msgs::msg::Lidar209ScanCH88>("lidar209_packets", 10);
    status_output_=
        this->create_publisher<lidar209_msgs::msg::Lidar209StatusCH176>("lidar209_status", 10);
  }else {
    throw std::runtime_error("Unknown LIDAR model: " + config_.model);
  }

  udp_packets_num=0;
  last_azimuth_ = -1;

  poll_thread_ = std::thread(&lidar209Driver::pollThread, this);

}

lidar209Driver::~lidar209Driver()
{
  exit_signal_.set_value();
  poll_thread_.join();
}

void lidar209Driver::pollThread()
{
  std::future_status status;

  do {
    poll_in_ch88();
    status = future_.wait_for(std::chrono::seconds(0));
  } while (status == std::future_status::timeout);
}

///** poll the device
// *
// *  @returns true unless end of file reached
// */
//bool lidar209Driver::poll(void)
//{
//  bool flag;
//  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
//  if (config_.model == "L209-2")
//  {

//    flag = poll_in_64g();
//  }
//  else if (config_.model == "L209-72")
//  {

//    flag = poll_in_ch72();
//  }
//  else if (config_.model == "L209-QS88")
//  {

//    flag = poll_in_ch88();
//  }
//  return true;
//}


bool lidar209Driver::poll_in_ch88(void)
{
  if (!config_.enabled) {
    // If we are not enabled exit once a second to let the caller handle
    // anything it might need to, such as if it needs to exit.
    std::this_thread::sleep_for(std::chrono::seconds(1));
    return true;
  }
  std::unique_ptr<lidar209_msgs::msg::Lidar209ScanCH88> scan =
      std::make_unique<lidar209_msgs::msg::Lidar209ScanCH88>();
  lidar209_msgs::msg::Lidar209StatusCH176  tmp_Status;
  int error_num=0;
 // scan->packets.reserve(config_.npackets);
  /*if( config_.cut_angle >= 0 && config_.cut_time <=0) //Cut at specific angle feature enabled
  {
    scan->packets.reserve(config_.npackets);
    lidar209_msgs::msg::Lidar209PacketCH88 tmp_packet_CH88;
    while(true)
    {
      while(true)
      {

        int rc = input_->getPacket(&tmp_packet_CH88, config_.time_offset);
        udp_packets_num++;
        if (rc == 0)
        {
          error_num=0;
          break;
        }
        if (rc < 0)
        {
          error_num++;
          tmp_Status.ui_msg_id=0x42570001;
          tmp_Status.ui_msg_datalength=14;
          tmp_Status.ul_msg_time=this->get_clock()->now().nanoseconds()/1000;
          tmp_Status.uc_lidar_id=0x01;
          tmp_Status.uc_lidar_state=0x2;
          tmp_Status.us_lidar_errcode=0x0;
          tmp_Status.us_check_sum=0x1;
          status_output_->publish(tmp_Status);
          return false;
        }
      }

      scan->packets.push_back(tmp_packet_CH88);
      udp_packets_num++;
      static int last_azimuth = -1;
      // Extract base rotation of first block in packet
      std::size_t azimuth_data_pos = 100*0+2;
      int azimuth = *( (u_int16_t*) (&tmp_packet_CH88.data[azimuth_data_pos]));

      // Handle overflow 35999->0
      if(azimuth<last_azimuth)
        last_azimuth-=36000;
      // Check if currently passing cut angle
      if(   last_azimuth != -1
            && last_azimuth < config_.cut_angle
            && azimuth >= config_.cut_angle )
      {
        last_azimuth = azimuth;
        break; // Cut angle passed, one full revolution collected
      }
      last_azimuth = azimuth;
    }
  }
  //Cut at specific timestamp feature enabled
  else */
  if ( config_.cut_time > 0)
  {
    lidar209_msgs::msg::Lidar209PacketCH88 tmp_packet_CH88;
    while(true)
    {
      while(true)
      {
        int rc = input_->getPacket(&tmp_packet_CH88, config_.time_offset);
        udp_packets_num++;
        if (rc == 0)
        {
          error_num=0;
          break;
        }
        if (rc < 0)
        {
          error_num++;
          tmp_Status.ui_msg_id=0x42570001;
          tmp_Status.ui_msg_datalength=14;
          tmp_Status.ul_msg_time=this->get_clock()->now().nanoseconds()/1000;
          tmp_Status.uc_lidar_id=0x01;
          tmp_Status.uc_lidar_state=0x2;
          tmp_Status.us_lidar_errcode=0x0;
          tmp_Status.us_check_sum=0x1;
          status_output_->publish(tmp_Status);
          return false;
        }
      }
      // publish message using time of last packet read
      scan->packets.push_back(tmp_packet_CH88);
      // Extract base timestamp of the packet
      const raw_packet_t_CH88 *raw = (const raw_packet_t_CH88 *) &tmp_packet_CH88.data[0];
      //      unsigned int timestamp = HLIPW(raw->revolution);

      byte tt[4];
      tt[0]=raw->revolution[3];
      tt[1]=raw->revolution[2];
      tt[2]=raw->revolution[1];
      tt[3]=raw->revolution[0];
      int timestamp = bytes4ToInt(tt);

      if ((timestamp%200000 <=554))
      {
        //ROS_INFO("Timestamp:%d",timestamp);
        time_error = abs(timestamp - last_scan_time);
        last_scan_time = timestamp;
        if(time_error>100000)
        {
         // ROS_INFO("Timestamp:%d",timestamp);
          break;
        }
        // break;//time passed, one full revolution collected
      }

    }
  }
  else // standard behaviour
  {
    // Since the lidar209 delivers data at a very high rate, keep
    // reading and publishing scans as fast as possible.
    scan->packets.resize(config_.npackets);
    for (int i = 0; i < config_.npackets; ++i)
    {
      while (true)
      {
        int rc = input_->getPacket(&scan->packets[i], config_.time_offset);
        udp_packets_num++;
        if (rc == 0)
        {
          error_num=0;
          break;
        }
        if (rc < 0)
        {
          error_num++;
          tmp_Status.ui_msg_id=0x42570001;
          tmp_Status.ui_msg_datalength=14;
          tmp_Status.ul_msg_time=this->get_clock()->now().nanoseconds()/1000;
          tmp_Status.uc_lidar_id=0x01;
          tmp_Status.uc_lidar_state=0x2;
          tmp_Status.us_lidar_errcode=0x0;
          tmp_Status.us_check_sum=0x1;
          status_output_->publish(tmp_Status);
          return false;
        }
      }
    }
  }
  std::size_t time_data_pos = 1380;
  unsigned char tt[4];
  tt[0]=scan->packets[0].data[time_data_pos+3];
  tt[1]=scan->packets[0].data[time_data_pos+2];
  tt[2]=scan->packets[0].data[time_data_pos+1];
  tt[3]=scan->packets[0].data[time_data_pos];

  tmp_Status.ui_msg_id=0x42570001;
  tmp_Status.ui_msg_datalength=14;
  tmp_Status.ul_msg_time=bytes4ToInt(tt);
  tmp_Status.uc_lidar_id=0x01;
  std::size_t status_data_pos = 1447;
  tmp_Status.uc_lidar_state=scan->packets[0].data[status_data_pos];
  tmp_Status.us_lidar_errcode=scan->packets[0].data[status_data_pos+1];
  tmp_Status.us_check_sum=0x2;

  status_output_->publish(tmp_Status);



  RCLCPP_DEBUG(this->get_logger(), "Publishing a full lidar209_CH88 scan.");
  builtin_interfaces::msg::Time stamp = scan->packets.back().stamp;
  scan->header.stamp = stamp;
  scan->header.frame_id = config_.frame_id;
  output_->publish(std::move(scan));

  diag_topic_->tick(stamp);

  return true;
}
int lidar209Driver::bytes4ToInt(unsigned char *b)
{
  unsigned int addr=b[0]&0xFF;
  addr|=((b[1]<<8)&0xFF00);
  addr|=((b[2]<<16)&0xFF0000);
  addr|=((b[3]<<24)&0xFF000000);
  return addr;
}
} // namespace lidar209_driver
RCLCPP_COMPONENTS_REGISTER_NODE(lidar209_driver::lidar209Driver)
